home *** CD-ROM | disk | FTP | other *** search
/ Developer CD Series 1995 August: Tool Chest / Dev.CD Aug 95 TC / Dev.CD Aug 95 TC.toast / Tool Chest / Development Tools & Languages / Dylan Related / Thomas / MacGambit⁄Thomas / MacGambit⁄Thomas Sources / Reference Notes / array.scm next >
Encoding:
Text File  |  1995-03-15  |  29.1 KB  |  785 lines  |  [TEXT/gamI]
  1. ; ----------------------------------------------------------------------------
  2. ; File:        array.scm
  3. ; Description: Array creation, manipulation functions.
  4. ; Author:      Raymond Laning at ART
  5. ; Created:     28-Apr-93
  6. ; Modified:    07-Dec-93  23:18:51 Raymond Laning
  7. ; Language:    Scheme
  8. ; Status:      Experimental (Use at your own risk)
  9. ;
  10. ;          (c) Copyright 1993, Advanced Robotic Technologies, Inc.
  11. ;              All Rights Reserved.
  12. ;
  13. ; ----------------------------------------------------------------------------
  14.  
  15. ;;;make-array makes an array of nxmx... entries where n is the row,
  16. ;;;m is the column, ... and so on - the last entry is the cell init value
  17. ;;;Example:  (make-array 3 4 "foo") makes a 3x4 array of "foo" entries
  18. (define (make-array . args)
  19.   (let* ((g1 (gensym))
  20.          (len (length args))
  21.          (dims (butlast args))
  22.          (vec (make-vector (apply * dims) (list-ref args (- len 1)))))
  23.     (put g1 'rank (- len 1))
  24.     (put g1 'dims dims)
  25.     (put g1 'vec vec)
  26.     (put g1 'offsets (make-vector (- len 1) 0))
  27.     (put g1 'newdims dims)
  28.     g1))
  29.  
  30. ;;;make-partition takes an existing array and lists of offsets
  31. ;;;(offsets are beginning and ending indices)
  32. ;;;and creates a pointer to the same data but with different
  33. ;;;dimensions (possibly different rank!)
  34. (define (make-partition . args)
  35.   (let* ((g1 (gensym))
  36.          (array (car args))
  37.          (previous-offsets (get array 'offsets))
  38.          (offset-list (cdr args))
  39.          (new-rank (length offset-list))
  40.          (vec (get array 'vec))
  41.          (oldims (get array 'dims)))
  42.     (put g1 'vec vec)
  43.     (put g1 'rank new-rank)
  44.     (put g1 'dims oldims)
  45.     (do ((offs offset-list (cdr offs))
  46.          (i 0 (+ i 1))
  47.          (newdims (list))
  48.          (newoffs (make-vector new-rank 0)))
  49.         ((null? offs) (put g1 'offsets newoffs)
  50.                       (put g1 'newdims (reverse newdims)))
  51.       (cond ((< (+ (cadar offs) (vector-ref previous-offsets i))
  52.                  (list-ref oldims i))
  53.              (vector-set! newoffs i (+ (caar offs)
  54.                                        (vector-ref previous-offsets i)))
  55.              (set! newdims (cons (- (cadar offs) (caar offs) -1) newdims)))
  56.             (#t (error "Bad partition dims:" offset-list oldims))))
  57.     g1))
  58.  
  59. ;;;copy partition makes a new array of dimensions (arg2-arg1)x(arg3-arg4)x...
  60. ;;;the partition should be of the same rank as the source
  61. (define (copy-partition . args)
  62.   (let* ((source (car args))
  63.          (limits (cdr args))
  64.          (dimensions (get source 'newdims))
  65.          (maxrow (car dimensions))
  66.          (newdims (map (lambda (argls) (- (cadr argls) (car argls) -1)) limits))
  67.          (rank (length newdims))
  68.          (dest (apply make-array (append newdims (list 0.0)))))
  69.     (case rank
  70.       ((0) (array-set! dest 0 (array-ref source 0)))
  71.       ((1) (do ((i 0 (+ i 1))
  72.                 (sourcei (caar limits) (+ 1 sourcei)))
  73.                ((> sourcei (cadar limits)))
  74.              (array-set! dest i (array-ref source (modulo sourcei maxrow)))))
  75.       ((2) (do ((i 0 (+ i 1))
  76.                 (sourcei (caar limits) (+ 1 sourcei)))
  77.                ((> sourcei (cadar limits)))
  78.              (do ((j 0 (+ j 1))
  79.                   (sourcej (caadr limits) (+ 1 sourcej)))
  80.                  ((> sourcej (cadadr limits)))
  81.                (array-set! dest i j (array-ref
  82.                                      source (modulo sourcei maxrow) sourcej)))))
  83.       ((3) (do ((i 0 (+ i 1))
  84.                 (sourcei (caar limits) (+ 1 sourcei)))
  85.                ((> sourcei (cadar limits)))
  86.              (do ((j 0 (+ j 1))
  87.                   (sourcej (caadr limits) (+ 1 sourcej)))
  88.                  ((> sourcej (cadadr limits)))
  89.                (do ((k 0 (+ k 1))
  90.                     (sourcek (caar (cddr limits)) (+ 1 sourcek)))
  91.                    ((> sourcek (cadar (cddr limits))))
  92.                  (array-set! dest i j k
  93.                       (array-ref
  94.                        source (modulo sourcei maxrow) sourcej sourcek))))))
  95.       (else (error "copy-partition doesn't do rank:" rank)))
  96.     dest))
  97.  
  98. (define (copy-partition-homogeneous source lim1 lim2)
  99.   (let* ((dimensions (get source 'newdims))
  100.          (maxrow (car dimensions))
  101.          (newdim2 (- (cadr lim2) (car lim2) -1))
  102.          (dest (make-array (- (cadr lim1) (car lim1)) newdim2 1.0)))
  103.     (do ((i 0 (+ i 1))
  104.          (sourcei (car lim1) (+ 1 sourcei)))
  105.         ((>= sourcei (cadr lim1)) dest)
  106.       (do ((j 0 (+ j 1))
  107.            (sourcej (car lim2) (+ 1 sourcej)))
  108.           ((> sourcej (cadr lim2)))
  109.         (array-set! dest i j (array-ref
  110.                               source (modulo sourcei maxrow) sourcej))))))
  111.  
  112. ;;;Array ref takes an array and the rows,columnsl,... and returns
  113. ;;;the values of that cell
  114. (define (array-ref . args)
  115.   (let* ((array (car args))
  116.          (refs (cdr args))
  117.          (dimensions (get array 'dims))
  118.          (dims-to-use (get array 'newdims))
  119.          (offsets (get array 'offsets))
  120.          (rank (get array 'rank)))
  121.     (cond ((and (= (length refs) rank)
  122.                 (do ((dims dims-to-use (cdr dims))
  123.                      (rfs refs (cdr rfs))
  124.                      (ok #t))
  125.                     ((or (not ok) (null? dims)) ok)
  126.                   (if (> (car rfs) (- (car dims) 1)) (set! ok #f) #t)))
  127.            (do ((i (- rank 1) (- i 1))
  128.                 (j 0 (+ j 1))
  129.                 (product 1)
  130.                 (index 0))
  131.                ((< i 0) (vector-ref (get array 'vec) index))
  132.              (set! index (+ index (* (+ (vector-ref offsets i)
  133.                                         (list-ref refs i))
  134.                                      product)))
  135.              (set! product (* product (list-ref dimensions i)))))
  136.           ((= (length refs) rank)
  137.            (error "Array subscript out of bounds:" refs dims-to-use))
  138.           (#t (error "Array reference not same rank as array:" refs rank)))))
  139.  
  140. ;;;array-set! is like array-ref:  given an array, the cell row,column...
  141. ;;;and a value, that cell is set to that value
  142. (define (array-set! . args)
  143.   (let* ((array (car args))
  144.          (val (list-ref args (- (length args) 1)))
  145.          (refs (butlast (cdr args)))
  146.          (dimensions (get array 'dims))
  147.          (rank (get array 'rank)))
  148.     (cond ((and (= (length refs) rank)
  149.                 (do ((dims dimensions (cdr dims))
  150.                      (rfs refs (cdr rfs))
  151.                      (ok #t))
  152.                     ((or (not ok) (null? dims)) ok)
  153.                   (if (> (car rfs) (- (car dims) 1)) (set! ok #f) #t)))
  154.            (do ((i (- rank 1) (- i 1))
  155.                 (j 0 (+ j 1))
  156.                 (product 1)
  157.                 (index 0))
  158.                ((< i 0) (vector-set! (get array 'vec) index val))
  159.              (set! index (+ index (* (list-ref refs i) product)))
  160.              (set! product (* product (list-ref dimensions i)))))
  161.           ((= (length refs) rank)
  162.            (error "Array subscript out of bounds:" refs dimensions))
  163.           (#t (error "Array reference not same rank as array:" refs rank)))))
  164.  
  165. ;;;takes an nxm list and returns an nxm array (n is rows, m is columns)
  166. (define (array! . args)
  167.   (let* ((dim-n (length args))
  168.          (dim-m (length (car args)))
  169.          (outarr (make-array dim-n dim-m 0.0)))
  170.     (do ((i 0 (+ i 1))
  171.          (rows args (cdr rows))
  172.          (row (car args)))
  173.         ((>= i dim-n) outarr)
  174.       (set! row (car rows)) 
  175.       (do ((j 0 (+ j 1)))
  176.           ((>= j dim-m))
  177.         (array-set! outarr i j (list-ref row j))))))
  178.  
  179. ;;;takes an list and returns an 1D array
  180. (define (oneDarray! . args)
  181.   (let* ((dim-n (length args))
  182.          (outarr (make-array dim-n 0.0)))
  183.     (do ((i 0 (+ i 1))
  184.          (thing args (cdr thing)))
  185.         ((>= i dim-n) outarr)
  186.       (array-set! outarr i (car thing)))))
  187.  
  188. ;;;copy-array takes a source array and copies it to a new,congruent
  189. ;;;(same number of rows and columns) destination array
  190. (define (copy-array source)
  191.   (let* ((dimss (get source 'dims))
  192.          (destination (apply make-array (append dimss (list 0.0))))
  193.          (vecs (get source 'vec))
  194.          (vecd (get destination 'vec)))
  195.     (do ((i 0 (+ i 1))
  196.          (len (vector-length vecs)))
  197.         ((>= i len))
  198.       (vector-set! vecd i (vector-ref vecs i)))
  199.     (put destination 'newdims (get source 'newdims))
  200.     (put destination 'offsets (get source 'offsets))
  201.     destination))
  202.  
  203. (define (transpose array)
  204.   (let ((dim (get array 'dims))
  205.         (rank (get array 'rank)))
  206.     (case rank
  207.       ((1)
  208.        (do ((i 0 (+ i 1))
  209.             (result (make-array 1 (car dim) 0.0)))
  210.            ((>= i (car dim)) result)
  211.          (array-set! result 0 i (array-ref array i))))
  212.       ((2)
  213.        (do ((i 0 (+ i 1))
  214.             (result (make-array (cadr dim) (car dim) 0.0)))
  215.            ((>= i (car dim)) result)
  216.          (do ((j 0 (+ j 1)))
  217.              ((>= j (cadr dim)))
  218.            (array-set! result j i (array-ref array i j)))))
  219.       (else (error "transpose can't do rank" rank)))))
  220.  
  221. (define (array-norm arr lastp)
  222.   (let* ((rank (get arr 'rank))
  223.          (dim1 (car (get arr 'newdims))))
  224.     (if lastp #t (set! dim1 (- dim1 1)))
  225.     (case rank
  226.       ((1) (do ((i 0 (+ i 1))
  227.                 (accum 0))
  228.                ((>= i dim1) (sqrt accum))
  229.              (set! accum (+ accum (* (array-ref arr i) (array-ref arr i))))))
  230.       (else (error "can't do norm of rank:" rank)))))
  231.  
  232. ;;;Normalize normalizes a vector in place(!): returns the vector
  233. ;;;of magnitude 1.0; lastp allows the last (homogeneous) coord to be ignored
  234. (define (normalize vec lastp)
  235.   (let* ((dim1 (car (get vec 'dims)))
  236.          (norm (array-norm vec lastp)))
  237.     (if lastp #t (set! dim1 (- dim1 1)))
  238.     (do ((i 0 (+ i 1)))
  239.         ((>= i dim1) vec)
  240.       (array-set! vec i (/ (array-ref vec i) norm)))))
  241.  
  242. ;;;scalar-mult returns a new array which is the source array * num
  243. (define (scalar-mult num arr)
  244.   (let* ((dims (get arr 'dims))
  245.          (newarr (copy-array arr)))
  246.     (do ((i 0 (+ i 1))
  247.          (vec (get newarr 'vec)))
  248.         ((>= i (vector-length vec)) newarr)
  249.       (vector-set! vec i (* num (vector-ref vec i))))))
  250.  
  251. (define (cross-product vec1 vec2)
  252.   (if (and (>= (car (get vec1 'dims)) 3) (>= (car (get vec2 'dims)) 3))
  253.     (let ((outarr (make-array 3 0.0)))
  254.       (array-set! outarr 0 (- (* (array-ref vec1 1) (array-ref vec2 2))
  255.                               (* (array-ref vec1 2) (array-ref vec2 1))))
  256.       (array-set! outarr 1 (- (* (array-ref vec1 2) (array-ref vec2 0))
  257.                               (* (array-ref vec1 0) (array-ref vec2 2))))
  258.       (array-set! outarr 2 (- (* (array-ref vec1 0) (array-ref vec2 1))
  259.                               (* (array-ref vec1 1) (array-ref vec2 0))))
  260.       outarr)
  261.     (error "Non-3D vector passed to cross-product:" vec1 vec2)))
  262.  
  263. (define (dot-product vec1 vec2)
  264.   (let* ((dim1 (car (get vec1 'newdims)))
  265.          (dim2 (car (get vec2 'newdims))))
  266.     (if (= dim1 dim2)
  267.       (do ((i 0 (+ i 1))
  268.            (accum 0.0))
  269.           ((>= i dim1) (make-array 1 accum))
  270.         (set! accum (+ accum (* (array-ref vec1 i) (array-ref vec2 i))))))))
  271.  
  272.  
  273. ;;;multiplies a 1-dimensional array times a two-dimensional array
  274. ;;;(here we call a 1-dim array a vector although it's not)
  275. ;;;it returns a 1D array
  276. (define (vecarrmult vector array)
  277.   (let* ((dim1 (get array 'newdims))
  278.          (dim2 (car (get vector 'newdims)))
  279.          (vec (get vector 'vec))
  280.          (a (get array 'vec))
  281.          (destination (make-array (cadr dim1) 0.0)))
  282.     (if (= (car dim1) dim2)
  283.       (do ((i 0 (+ i 1))
  284.            (sum 0 0))
  285.           ((>= i (cadr dim1)))
  286.         (do ((j 0 (+ 1 j)))
  287.             ((>= j dim2))
  288.           (set! sum (+ sum (* (array-ref array j i) (vector-ref vec j)))))
  289.         (array-set! destination i sum))
  290.       (error "Non-matching array dim, vector passed to vecarrmult:"
  291.              dim1 dim2))
  292.     destination))
  293.  
  294. ;;;this guy actually multiplies a vector times a 2D array and
  295. ;;;returns a vector result
  296. (define (vecarraymult vector array)
  297.   (let* ((dim1 (get array 'newdims))
  298.          (dim2 (vector-length vector))
  299.          (a (get array 'vec))
  300.          (destination (make-vector (cadr dim1) 0.0)))
  301.     (if (= (car dim1) dim2)
  302.       (do ((i 0 (+ i 1))
  303.            (sum 0 0))
  304.           ((>= i (cadr dim1)))
  305.       (do ((j 0 (+ j 1)))
  306.           ((>= j (car dim1)))
  307.           (set! sum (+ sum (* (array-ref array j i) (vector-ref vector j)))))
  308.       (vector-set! destination i sum))
  309.       (error "Non-matching array dim, vector passed to vecarraymult:"
  310.              dim1 dim2))
  311.     destination))
  312.  
  313. (define (array-product arr1 arr2)
  314.   (let* ((dim1 (get arr1 'newdims))
  315.          (dim2 (get arr2 'newdims))
  316.          (len1 (length dim1))
  317.          (len2 (length dim2))
  318.          (d11 (car dim1))
  319.          (d12 (if (= len1 1) 1 (cadr dim1)))
  320.          (d21 (car dim2))
  321.          (d22 (if (= len2 1) 1 (cadr dim2)))
  322.          (destination (make-array d11 d22 1.0)))
  323.     (if (= d12 d21)
  324.       (do ((i 0 (+ 1 i)))
  325.           ((>= i d11))
  326.         (do ((k 0 (+ 1 k))
  327.              (sum 0 0))
  328.             ((>= k d22))
  329.           (do ((j 0 (+ 1 j))
  330.                (temp 0.0))
  331.               ((>= j d12))
  332.             (if (= len2 1) (set! temp (array-ref arr2 k))
  333.                            (set! temp (array-ref arr2 j k)))
  334.             (set! sum (+ sum (* (array-ref arr1 i j) temp))))
  335.           (array-set! destination i k sum)))
  336.       (error "Non-matching array dimensions passed to array-product:"
  337.              dim1 dim2))
  338.     destination))
  339.  
  340. (define (tensor-product array tensor)
  341.   (let* ((dim1 (get array 'newdims))
  342.          (dim2 (get tensor 'newdims))
  343.          (d11 (car dim1))
  344.          (d12 (cadr dim1))
  345.          (d21 (car dim2))
  346.          (d22 (cadr dim2))
  347.          (d23 (caddr dim2))
  348.          (destination (make-array d11 d22 d23 1.0)))
  349.     (if (= d12 d21)
  350.       (do ((i 0 (+ 1 i)))
  351.           ((>= i d11))
  352.         (do ((k 0 (+ 1 k))
  353.              (sum 0 0)
  354.              (sumarray (make-array d23 0.0) (make-array d23 0.0)))
  355.             ((>= k d22))
  356.           (do ((j 0 (+ 1 j)))
  357.               ((>= j d12))
  358.             (do ((l 0 (+ l 1)))
  359.                 ((>= l d23))
  360.               (array-set! sumarray l
  361.                           (+ (array-ref sumarray l)
  362.                              (* (array-ref array i j)
  363.                                 (array-ref tensor j k l))))))
  364.           (do ((l 0 (+ l 1)))
  365.               ((>= l d23))
  366.             (array-set! destination i k l (array-ref sumarray l)))))
  367.       (error "Non-matching array dimensions passed to tensor-product:"
  368.              dim1 dim2))
  369.     destination))
  370.  
  371. (define (tensor-product2 tensor tarray)
  372.   (let* ((dim1 (get tarray 'newdims))
  373.          (dim2 (get tensor 'newdims))
  374.          (d21 (car dim1))
  375.          (d22 (cadr dim1))
  376.          (d11 (car dim2))
  377.          (d12 (cadr dim2))
  378.          (d13 (caddr dim2))
  379.          (destination (make-array d11 d22 d13 1.0)))
  380.     (if (= d12 d21)
  381.       (do ((i 0 (+ 1 i)))
  382.           ((>= i d11))
  383.         (do ((k 0 (+ 1 k))
  384.              (sum 0 0)
  385.              (sumarray (make-array d13 0.0) (make-array d13 0.0)))
  386.             ((>= k d22))
  387.           (do ((j 0 (+ 1 j)))
  388.               ((>= j d12))
  389.             (do ((l 0 (+ l 1)))
  390.                 ((>= l d13))
  391.               (array-set! sumarray l
  392.                           (+ (array-ref sumarray l)
  393.                              (* (array-ref tensor i j l)
  394.                                 (array-ref tarray j k))))))
  395.           (do ((l 0 (+ l 1)))
  396.               ((>= l d13))
  397.             (array-set! destination i k l (array-ref sumarray l)))))
  398.       (error "Non-matching array dimensions passed to tensor-product2:"
  399.              dim1 dim2))
  400.     destination))
  401.  
  402. (define (tensor-partial-transpose tensor)
  403.   (let ((dim (get tensor 'newdims)))
  404.     (do ((i 0 (+ i 1))
  405.          (result (make-array (cadr dim) (car dim) (caddr dim) 0.0)))
  406.         ((>= i (car dim)) result)
  407.       (do ((j 0 (+ j 1)))
  408.           ((>= j (cadr dim)))
  409.         (do ((k 0 (+ k 1)))
  410.             ((>= k (caddr dim)))
  411.           (array-set! result j i k (array-ref tensor i j k)))))))
  412.  
  413. (define (vectenmult vector tensor)
  414.   (let* ((d1 (car (get vector 'newdims)))
  415.          (tendims (get tensor 'newdims))
  416.          (d21 (car tendims))
  417.          (dest (apply make-array (append (cdr tendims) (list 0.0)))))
  418.     (if (= d1 d21)
  419.       (do ((j 0 (+ j 1)))
  420.           ((>= j (cadr tendims)) dest)
  421.         (do ((i 0 (+ i 1)))
  422.             ((>= i (car tendims)))
  423.           (do ((k 0 (+ k 1)))
  424.               ((>= k (caddr tendims)))
  425.             (array-set! dest j k (+ (* (array-ref vector i)
  426.                                        (array-ref tensor i j k))
  427.                                     (array-ref dest j k))))
  428.           ))
  429.       (error "Non-matching indices for vectenmult:" d1 d21))))
  430.  
  431. (define (point-distance array index1 index2)
  432.   (do ((i 0 (+ i 1))
  433.        (dx 0.0) (onedist 0.0))
  434.       ((>= i 3) (sqrt dx))
  435.     (set! onedist (- (array-ref array index1 i) (array-ref array index2 i)))
  436.     (set! dx (+ dx (* onedist onedist)))))
  437.  
  438. (define (removit element alist)
  439.   (do ((lesslist alist (cdr lesslist))
  440.        (outlist (list)))
  441.       ((null? lesslist) (reverse outlist))
  442.     (if (equal? element (car lesslist))
  443.       #t
  444.       (set! outlist (cons (car lesslist) outlist)))))
  445.  
  446. (define (arraymult arr1 arr2)
  447.   (array* arr1 arr2))
  448.  
  449. ;;;array* multiplies two arrays of appropriate dimension and returns a
  450. ;;;(new) product array
  451. (define (array* arr1 arr2)
  452.   (let ((numberp1 (number? arr1))
  453.         (numberp2 (number? arr2)))
  454.     (cond ((and numberp1 numberp2) (* arr1 arr2))
  455.           (numberp1 (scalar-mult arr1 arr2))
  456.           (numberp2 (scalar-mult arr2 arr1))
  457.           (#t
  458.            (let* ((r1 (get arr1 'rank))
  459.                   (r2 (get arr2 'rank))
  460.                   (dims 0) (newdims 0)
  461.                   (out
  462.                    (case r1
  463.                      ((1)
  464.                       (case r2
  465.                         ((1) (dot-product arr1 arr2))
  466.                         ((2) (vecarrmult arr1 arr2))
  467.                         ((3) (vectenmult arr1 arr2))
  468.                         (else (error "array* can't do ranks:" r1 r2))))
  469.                      ((2)
  470.                       (case r2
  471.                         ((1 2) (array-product arr1 arr2))
  472.                         ((3) (tensor-product arr1 arr2))
  473.                         (else (error "array* can't do ranks:" r1 r2))))
  474.                      ((3)
  475.                       (case r2
  476.                         ((1) (error "array* can't do ranks:" r1 r2))
  477.                         ((2) (tensor-product2 arr1 arr2))
  478.                         (else (error "array* can't do ranks:" r1 r2)))))))
  479.              out)))))
  480.  
  481. ;;;array- subtracts two congruent arrays and returns a difference array
  482. (define (array- arr1 arr2)
  483.   (let* ((r1 (get arr1 'rank))
  484.          (r2 (get arr2 'rank))
  485.          (dim1 (get arr1 'newdims))
  486.          (dim2 (get arr2 'newdims))
  487.          (dest (apply make-array (append dim1 (list 0.0)))))
  488.     (if (= r1 r2)
  489.       (if (equal? dim1 dim2)
  490.         (case r1
  491.           ((1) (do ((i 0 (+ i 1)))
  492.                    ((>= i (car dim1)) dest)
  493.                  (array-set! dest i (- (array-ref arr1 i) (array-ref arr2 i)))))
  494.           ((2) (do ((i 0 (+ i 1)))
  495.                    ((>= i (car dim1)) dest)
  496.                  (do ((j 0 (+ j 1)))
  497.                      ((>= j (cadr dim1)))
  498.                    (array-set! dest i j (- (array-ref arr1 i j)
  499.                                            (array-ref arr2 i j))))))
  500.           (else (error "Can't array- rank:" r1)))
  501.         (error "non-congruent arrays in array-:" dim1 dim2))
  502.       (error "Different rank arrays in array-:" r1 r2))))
  503.  
  504. ;;;like array-, only adds
  505. (define (array+ arr1 arr2)
  506.   (let* ((r1 (get arr1 'rank))
  507.          (r2 (get arr2 'rank))
  508.          (dim1 (get arr1 'newdims))
  509.          (dim2 (get arr2 'newdims))
  510.          (dest (apply make-array (append dim1 (list 0.0)))))
  511.     (if (= r1 r2)
  512.       (if (equal? dim1 dim2)
  513.         (case r1
  514.           ((1) (do ((i 0 (+ i 1)))
  515.                    ((>= i (car dim1)) dest)
  516.                  (array-set! dest i (+ (array-ref arr1 i) (array-ref arr2 i)))))
  517.           ((2) (do ((i 0 (+ i 1)))
  518.                    ((>= i (car dim1)) dest)
  519.                  (do ((j 0 (+ j 1)))
  520.                      ((>= j (cadr dim1)))
  521.                    (array-set! dest i j (+ (array-ref arr1 i j)
  522.                                            (array-ref arr2 i j))))))
  523.           (else (error "Can't array+ rank:" r1)))
  524.         (error "non-congruent arrays in array+:" dim1 dim2))
  525.       (error "Different rank arrays in array+:" r1 r2))))
  526.  
  527. ;;;;if a dimension is degenerate, crunch it down (reduce rank)
  528. (define (reduce-rank out)
  529.   (let ((dims (get out 'dims))
  530.         (newdims (get out 'newdims)))
  531.     (set! dims (removit 0 (removit 1 dims)))
  532.     (set! newdims (removit 0 (removit 1 newdims)))
  533.     (put out 'dims dims)
  534.     (put out 'newdims newdims)
  535.     (put out 'rank (length newdims))
  536.     out))
  537.  
  538.  
  539. (define (print-array arr)
  540.   (let* ((rank (get arr 'rank))
  541.          (dims (get arr 'newdims))
  542.          (columns (if (not (null? dims)) (car dims))))
  543.     (cond ((= rank 0)
  544.            (format #t "|~s|~%" (vector-ref (get arr 'vec) 0)))
  545.           ((= rank 1)
  546.            (format #t "|")
  547.            (do ((j 0 (+ j 1)))
  548.                ((>= j columns))
  549.              (format #t "~7 " (array-ref arr j)))
  550.            (format #t "|~%"))
  551.           ((= rank 2)
  552.            (do ((j 0 (+ j 1))
  553.                 (rows (cadr dims)))
  554.                ((>= j columns))
  555.              (format #t "|")
  556.              (do ((i 0 (+ i 1)))
  557.                  ((>= i rows))
  558.                (format #t "~7 " (array-ref arr j i)))
  559.              (format #t "|~%")))
  560.           ((= rank 3)
  561.            (do ((j 0 (+ j 1))
  562.                 (rows (cadr dims))
  563.                 (third (caddr dims)))
  564.                ((>= j columns))
  565.              (format #t "|")
  566.              (do ((k 0 (+ k 1)))
  567.                  ((>= k third))
  568.                (format #t " |")
  569.                (do ((i 0 (+ i 1)))
  570.                    ((>= i rows))
  571.                  (format #t "~7 " (array-ref arr j i k)))
  572.                (format #t "|"))
  573.              (format #t "|~%")))
  574.           (else (error "Don't know how to print array of rank" rank)))
  575.     arr))
  576.  
  577. (define (print-array-partial . args)
  578.   (let* ((arr (car args))
  579.          (limits (cdr args))
  580.          (rank (get arr 'rank))
  581.          (dims (get arr 'newdims))
  582.          (columns (if (not (null? dims)) (car dims))))
  583.     (cond ((not (= (length limits) rank))
  584.            (error "limits not equal to rank of array" limits rank))
  585.           ((= rank 0)
  586.            (format #t "|~s|~%" (vector-ref (get arr 'vec) 0)))
  587.           ((= rank 1)
  588.            (format #t "|")
  589.            (do ((j (caar limits) (+ j 1)))
  590.                ((> j (cadar limits)))
  591.              (format #t "~7 " (array-ref arr j)))
  592.            (format #t "|~%"))
  593.           ((= rank 2)
  594.            (do ((j (caar limits) (+ j 1))
  595.                 (rows (cadr dims)))
  596.                ((> j (cadar limits)))
  597.              (format #t "|")
  598.              (do ((i (car (cadr limits)) (+ i 1)))
  599.                  ((> i (cadr (cadr limits))))
  600.                (format #t "~7 " (array-ref arr j i)))
  601.              (format #t "|~%")))
  602.           ((= rank 3)
  603.            (do ((j (caar limits) (+ j 1))
  604.                 (rows (cadr dims))
  605.                 (third (caddr dims)))
  606.                ((> j (cadar limits)))
  607.              (format #t "|")
  608.              (do ((k (car (cadr limits)) (+ k 1)))
  609.                  ((> k (cadr (cadr limits))))
  610.                (format #t " |")
  611.                (do ((i (car (caddr limits)) (+ i 1)))
  612.                    ((> i (cadr (caddr limits))))
  613.                  (format #t "~7 " (array-ref arr j i k)))
  614.                (format #t "|"))
  615.              (format #t "|~%")))
  616.           (else (error "Don't know how to print array of rank" rank)))
  617.     arr))
  618.  
  619. (define (identity-matrix . args)
  620.   (let* ((dims (or (and (not (null? args)) (car args)) 4))
  621.          (mat (make-array dims dims 0.0)))
  622.     (do ((i 0 (+ i 1)))
  623.         ((>= i dims) mat)
  624.       (array-set! mat i i 1.0))))
  625.  
  626. (define (yaw mat angle)
  627.   (let ((yawmat (make-array 4 4 0.0))
  628.         (cosa (cos (degrees-to-radians angle)))
  629.         (sina (sin (degrees-to-radians angle))))
  630.     (array-set! yawmat 0 0 cosa)
  631.     (array-set! yawmat 0 1 (- sina))
  632.     (array-set! yawmat 1 0 sina)
  633.     (array-set! yawmat 1 1 cosa)
  634.     (array-set! yawmat 2 2 1.0)
  635.     (array-set! yawmat 3 3 1.0)
  636.     (arraymult mat yawmat)))
  637.  
  638. (define (pitch mat angle)
  639.   (let ((pitchmat (make-array 4 4 0.0))
  640.         (cosa (cos (degrees-to-radians angle)))
  641.         (sina (sin (degrees-to-radians angle))))
  642.     (array-set! pitchmat 0 0 cosa)
  643.     (array-set! pitchmat 0 2 sina)
  644.     (array-set! pitchmat 2 0 (- sina))
  645.     (array-set! pitchmat 2 2 cosa)
  646.     (array-set! pitchmat 1 1 1.0)
  647.     (array-set! pitchmat 3 3 1.0)
  648.     (arraymult mat pitchmat)))
  649.  
  650. (define (roll mat angle)
  651.   (let ((rollmat (make-array 4 4 0.0))
  652.         (cosa (cos (degrees-to-radians angle)))
  653.         (sina (sin (degrees-to-radians angle))))
  654.     (array-set! rollmat 2 2 cosa)
  655.     (array-set! rollmat 1 1 cosa)
  656.     (array-set! rollmat 1 2 (- sina))
  657.     (array-set! rollmat 2 1 sina)
  658.     (array-set! rollmat 0 0 1.0)
  659.     (array-set! rollmat 3 3 1.0)
  660.     (arraymult mat rollmat)))
  661.  
  662. (define (rotation-matrix-about-vector vector angle)
  663.   (let* ((mat (make-array 4 4 0.0))
  664.          (mag (list-magnitude vector))
  665.          (n1 (/ (car vector) mag))
  666.          (n2 (/ (cadr vector) mag))
  667.          (n3 (/ (caddr vector) mag))
  668.          (cosa (cos angle))
  669.          (sina (sin angle)))
  670.     (array-set! mat 0 0 (+ (* n1 n1) (* (- 1 (* n1 n1)) cosa)))
  671.     (array-set! mat 0 1 (+ (* n1 n2 (- 1 cosa)) (* n3 sina)))
  672.     (array-set! mat 0 2 (- (* n1 n3 (- 1 cosa)) (* n2 sina)))
  673.     (array-set! mat 1 0 (- (* n1 n2 (- 1 cosa)) (* n3 sina)))
  674.     (array-set! mat 1 1 (+ (* n2 n2) (* (- 1 (* n2 n2)) cosa)))
  675.     (array-set! mat 1 2 (+ (* n2 n3 (- 1 cosa)) (* n1 sina)))
  676.     (array-set! mat 2 0 (+ (* n1 n3 (- 1 cosa)) (* n2 sina)))
  677.     (array-set! mat 2 1 (- (* n2 n3 (- 1 cosa)) (* n1 sina)))
  678.     (array-set! mat 2 2 (+ (* n3 n3) (* (- 1 (* n3 n3)) cosa)))
  679.     (array-set! mat 3 3 1.0)
  680.     mat))
  681.  
  682. (define (translate mat x y z)
  683.   (array-set! mat 3 0 (+ (array-ref mat 3 0) x))
  684.   (array-set! mat 3 1 (+ (array-ref mat 3 1) y))
  685.   (array-set! mat 3 2 (+ (array-ref mat 3 2) z))
  686.   mat)
  687.  
  688. (define (arr-extrema array1)
  689.   (let* ((dims (get array1 'dims))
  690.          (dim1 (car dims))
  691.          (dim2 (cadr dims)))
  692.     (do ((ind 0 (+ ind 1))
  693.          (extremes (do ((i 0 (+ i 1))
  694.                         (vec (make-vector (* 2 dim2) -1.e300)))
  695.                        ((>= i dim2) vec)
  696.                      (vector-set! vec i 1.e300))))
  697.         ((>= ind dim1) extremes)
  698.       (do ((i 0 (+ i 1)))
  699.           ((>= i dim2))
  700.         (if (< (array-ref array1 ind i) (vector-ref extremes i))
  701.           (vector-set! extremes i (array-ref array1 ind i)))
  702.         (if (> (array-ref array1 ind i) (vector-ref extremes (+ i dim2)))
  703.           (vector-set! extremes (+ i dim2) (array-ref array1 ind i)))))))
  704.  
  705. (define (average-arr-extrema extvec)
  706.   (let ((halflen (integer-coerce (/ (vector-length extvec) 2))))
  707.     (do ((i 0 (+ i 1))
  708.          (outlist (list)))
  709.         ((>= i halflen) (reverse outlist))
  710.       (set! outlist (cons (/ (+ (vector-ref extvec i)
  711.                                 (vector-ref extvec (+ i halflen)))
  712.                              2)
  713.                           outlist)))))
  714.  
  715.  
  716. (define *effector* '(0.0 0.0 0.0 0.0 0.0 0.0))
  717.  
  718. (define *inv-effector-transform* (list))
  719.  
  720.  
  721. ;;; Given a non-singular matrix arr1, returns a new inverse matrix of the same
  722. ;;; dimension.  The values in array arr1 will be destroyed by the inversion
  723. ;;; algorithm.  It is an error if the matrix is not square.
  724.  
  725. (define (arrayinvert arr1)
  726.   (let* ((adim (get arr1 'dims))
  727.          (dim (car adim))
  728.          (odim (cadr adim)))
  729.     (if (= dim odim)
  730.       (let ((arr2 (identity-matrix dim))) ;; set up arr2 as identity matrix.
  731.  
  732.         (do ((diag 0 (+ diag 1))
  733.              (amax -1.0)
  734.              (imax 0))
  735.             ((>= diag dim))
  736.  
  737.           ;; find the maximal element in column diag.
  738.           (do ((i 0 (+ i 1))
  739.                (adiag 0))
  740.               ((>= i dim))
  741.             (set! adiag (abs (array-ref arr1 i diag)))
  742.             (if (>= adiag amax)
  743.               (begin
  744.                 (set! amax adiag)
  745.                 (set! imax i))))
  746.           
  747.           ;; swap row diag with the row containing the maximal element.
  748.           (do ((j 0 (+ j 1))
  749.                (tmp 0.0))
  750.               ((>= j dim))
  751.             (set! tmp (array-ref arr1 imax j))
  752.             (array-set! arr1 imax j (array-ref arr1 diag j))
  753.             (array-set! arr1 diag j tmp)
  754.             (set! tmp (array-ref arr2 imax j))
  755.             (array-set! arr2 imax j (array-ref arr2 diag j))
  756.             (array-set! arr2 diag j tmp))
  757.           
  758.           ;; scale row diag so that the value of the diagonal element is 1.0.
  759.           (do ((j 0 (+ j 1))
  760.                (s (/ 1.0 (array-ref arr1 diag diag))))
  761.               ((>= j dim))
  762.             (array-set! arr1 diag j (* (array-ref arr1 diag j) s))
  763.             (array-set! arr2 diag j (* (array-ref arr2 diag j) s)))
  764.           
  765.           ;; clear out the column above and below diagonal diag.
  766.           (do ((i 0 (+ i 1))
  767.                (return #f)
  768.                (s 0.0))
  769.               ((or return (>= i dim)))
  770.             (if (= i diag)
  771.               (set! return #t)
  772.               (begin
  773.                 (set! s (array-ref arr1 i diag))
  774.                 (do ((j 0 (+ j 1)))
  775.                     ((>= j dim))
  776.                   (array-set! arr1 i j (- (array-ref arr1 i j)
  777.                                           (* s (array-ref arr1 diag j))))
  778.                   (array-set! arr2 i j (- (array-ref arr2 i j)
  779.                                           (* s (array-ref arr2 diag j)))))))))
  780.         arr2
  781.         )
  782.       (error "A matrix passed to arrayinvert must be square:" dim dim0)
  783.       ))
  784.   )
  785.